/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#ifndef _PARTICLEMATH_H_
#define _PARTICLEMATH_H_

#include "linelib.h"

#ifndef PI
	#define PI	3.141592654
#endif

#define NUMBER_OF_MEASUREMENTS 10	// Number of virtual scan lines
#ifndef LASER_MAX_DIST2
	#define LASER_MAX_DIST2 (80.0*80.0) // The maximum distance of the laser measurement squared
#endif 



#define VMAP 0.2					// Map uncertainty
#define VANG 0.2					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.3				// "Probability" of the false measurement

#define VMEAS ((VMAP+VANG+VR)*(VMAP+VANG+VR)) // Whole measurement uncertainty

#define RANDOM_CACHE_SIZE 10000
    
    
    
namespace mcl{
    /**
		 * Definition of scan for Scan Match interface
		 * Uses the polar coordinate format (r,a) pairs. 
		 * Natural for sensor coordinates and used as sensor coordinates here!
		 */
		struct scan{
				public:
						float *r;  ///< Distance measurement vector [m]
						float *a;  ///< Angle measurement of distance measurement [rad] 
						int N;     ///< The number of measurements in vector
						scan(){
								N=0;
								r=NULL;
								a=NULL;
						}
						~scan(){
								if(r) free(r);
								if(a) free(a);
						}
						void allocate(int n){
								r = (float *) malloc(n*sizeof(float));
								a = (float *) malloc(n*sizeof(float));
						}
						void copy(struct scan &new_scan){
								new_scan.r = (float *) malloc(N*sizeof(float));
								new_scan.a = (float *) malloc(N*sizeof(float));
								new_scan.N = N;
								memcpy(new_scan.r,r,N*sizeof(float));
								memcpy(new_scan.a,a,N*sizeof(float));
						}
		};
	/**
		 * ScanMatch pose definition
		 * 2D-pose x,y and heading 
	 */
		struct pose{
				public:
						float x; ///<[m]
						float y; ///<[m]
						float a; ///<[rad]
						pose(){
								x=0;
								y=0;
								a=0;
						}
						void set(pose &p){
								x=p.x;
								y=p.y;
								a=p.a;
						}
						void set(float xx,float yy, float aa){
								x=xx;
								y=yy;
								a=aa;
						}
						~pose(){}
		
		};
}


struct TPoseParticle{
		public:
		float x;	// X - coordinate of the particle
		float y;	// Y - coordinate of the particle
		float a;	// Heading
		float p;	// Probability of the particle 
		float lik;	// Likelihood of the particle
		TPoseParticle(){
				x=0;
				y=0;
				a=0;
				p=0;
				lik=0;
		}
};


/**
* Class for containing all relevant information about the distribution 
**/
struct TPoseParticleDistribution{
		public:
				TPoseParticle *Particles;	// Particle distribution
				float Lik;					// Likelihood of the distribution
				float outLiers;			// Percentage of the outlier measurements in distribution
				int NumOfParticles;			// Number of particles in distribution
				int size;					// reserved memory Size 
				mcl:pose average;     // Distribution mean
				mcl::pose variance;   // Distribution variance
				bool isAvgSet;    // Tells if the distribution mean has been calculated
				
				///Constructor
				TPoseParticleDistribution(){
						Particles=NULL;
						Lik=0;
						outLiers=0;
						NumOfParticles=0;
						size=0;
						isAvgSet = false;
				}
				
				///Allocate memory for @num_particles particles
				allocate(int num_particles){
						Particles = (TPoseParticle *) malloc(size*sizeof(num_particles));
						size = num_particles;
						NumOfParticles=0;
						outLiers = 0;
						Lik=0;
				}
				///Frees the distribution memory
				mymyfree(){
						if(Particles!=NULL) free(Particles);
						size = 0; 
						NumOfParticles = 0;
				}
				
				///Get the mean values of the distribution
				///Returns mcl::pose
				mcl::pose getDistributionMean(){
						float sumX=0,sumY=0,sumA=0;
						int i;
						mcl::pose pos;
						
						isAvgSet = true;
						
						for(i=0;i<NumOfParticles;i++){
								sumX += Particles[i].x;
								sumY += Particles[i].y;
								sumA += Particles[i].a;
						}
						pos.x = sumX / (float) NumOfParticles;
						pos.y = sumY / (float) NumOfParticles;
						pos.a = sumA / (float) NumOfParticles;
						
						average = pos;
						
						return pos;
				}
				
				mcl::pose getDistributionVariances(){
						mcl::pose avg,var;
						int i;
						
						if(isAvgSet) avg = average;
						else
								avg = getDistributionMean();
						  
						var.x=0; var.y=0; var.a=0; 
						avg = calculateMeanPosition(PPD);

						for(i=0;i<PPD.NumOfParticles;i++){
								var.x += (Particles[i].x - avg.x)*(Particles[i].x - avg.x);
								var.y += (Particles[i].y - avg.y)*(Particles[i].y - avg.y);
								var.a += (Particles[i].a - avg.a)*(Particles[i].a - avg.a);
						}
	
						var.x = var.x / (PPD.NumOfParticles-1);
						var.y = var.y / (PPD.NumOfParticles-1);
						var.a = var.a / (PPD.NumOfParticles-1);
						
						variance = var;
						
						return var;
				}
				
				
} ;



float uniformRandom(void);
float normalRandom();
float normalRandomHeavy();
float getNormalRandomFromCache();
void newRandomSeed();

/**
* Calculates initial pose particle distribution. 
* Samples from normal distribution, thus the initial position is expected to 
* be somewhat known (e.g. within 3mx3m area with angle uncertainty of 10 deg if 
* approx. 800 particles is used)
*  
* TPose pos initial position estimate
* float Q[3] uncertainty (std) of the initial position
* Q[0] x - uncertainty
* Q[1] y - uncertainty
* Q[2] angle uncertainty
* int size: the number of particles in distribution
* 
* returns normally distributed particle distribution
* OBS! creates TPoseParticleDistribution	
*/
TPoseParticleDistribution getInitialDistribution(TPose initPos,
												 float Q[3], 
												 int size
												 );

TPose calculateMeanPosition(TPoseParticleDistribution PPD);

TPose calculateParticleVariances(TPoseParticleDistribution PPD);

TPoseParticleDistribution SIRupdate(TPoseParticleDistribution PPD);

TPoseParticleDistribution penaPredict(
									  TPoseParticleDistribution PPD,
									  float v[3],
									  float Q[3]);

TPoseParticleDistribution penaPredictCov(
										 TPoseParticleDistribution PPD,
										 float v[3],
										 float Q[4]);

TPoseParticleDistribution normalizeDistribution(TPoseParticleDistribution PPD);

TPoseParticleDistribution estimateInitialPosition( 
												  TPose initPose,
												  int size,
												  float v[3],
									   			  float Q[3],
									   			  TLineVector cMap,
												  float *scan);


/**********************************************************************
* getScanLines
* Returns line parameters (a,b,c) ax+by+c=0 for one scan taken from given position
*
* @param *scanLines		Output: The lineparameter representing the scan. 
*						Size should be 3*@number_of_measurements
* @param P				The position to which the lines are calculated
* @param number_of_measurements The number of lines in scan
* @param laserOffset	The offset between the robot coordinatesystem and laser coordinate system	
*
***************************************************/ 
void getScanLines(float *scanLines,
				  TPoseParticle P, 
				  int number_of_measurements,
				  float laserOffset);



/**
* Calculates virtual scan from line map
* This is improved algorithm from the one presented above
* 
* @param *r				Output: The range measurements. If not used set as NULL. 
*						The size should be same as @number_of_measurements
* @param *xyscan_rel	Output: The relative x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param *xyscan_abs	Output: The absolute x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param *scanLines		The calculated scanlines 
* @param Pos			The position where the robot is assumed to be
* @param map			The linemap
*
* @param number_of_measurements The number of laser beams to be calculated
* @param laserOffset	The angle difference between the robot coordinate system and laser coordinate system.
*						e.g. If your robot is travelling along X-axis in zero heading, the laser is probably 
*						pointing forward, thus the first measurement (0 angle from laser point of view) is from 
						-PI/2 from the robot perspective. In this case the offset is -PI/2
*
*/
int findVirtualScan(float *Scan, 
					 float *scanLines, 
					 TLineVector map,
					 TPoseParticle Pos,
					 int number_of_measurements);

int findVirtualScan2(float *r,
					  float *xyscan_rel,
					  float *xyscan_abs,
					  float *scanLines,
					  TPoseParticle Pos,
					  TLineVector map, 
					  int number_of_measurements,
					  float laserOffset);
//---------------------------------------------------------------------------//

TLineVector findVisibilityLines( 
					 TLineVector map,
					 TPoseParticle Pos
					 );

TPoseParticleDistribution calculateLikelihood(
											  TPoseParticleDistribution PPD, 
											  TLineVector Map,
											  float *Scan);

TPoseParticleDistribution MCLBasicCovStep(
										  TPoseParticleDistribution PPD,
										  float v[3],
										  float Q[4],
										  TLineVector cMap,
										  float *scan);

TPoseParticleDistribution MCLBasicStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan);

TPoseParticleDistribution MCLBasicStepNoSIR(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan);


TPoseParticleDistribution ASIRStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan);




/************************************************
* Calculates virtual scan from line map
* 
* @param *r				Output: The range measurements. If not used set as NULL. 
*						The size should be same as @number_of_measurements
* @param *xyscan_rel	Output: The relative x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param *xyscan_abs	Output: The absolute x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param Pos			The position where the robot is assumed to be
* @param map			The linemap
*
* @param number_of_measurements The number of laser beams to be calculated
* @param laserOffset	The angle difference between the robot coordinate system and laser coordinate system.
*						e.g. If your robot is travelling along X-axis in zero heading, the laser is probably 
*						pointing forward, thus the first measurement (0 angle from laser point of view) is from 
						-PI/2 from the robot perspective. In this case the offset is -PI/2
*
*/
int VirtualScan(float *r,
					  float *xyscan_rel,
					  float *xyscan_abs,
					  TPoseParticle Pos,
					  TLineVector map, 
					  int number_of_measurements,
					  float laserOffset);

/** 
* Saves the scans to file
*/
void testVirtualScan(TPoseParticle Pos,
					  TLineVector map, 
					  int number_of_measurements,
					  float laserOffset);




#endif
